package net.openadventure.pathfinding;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.List;
import java.util.TreeSet;

import net.openadventure.State;
import util.Point;

public class Pathfinder {

	public static List<GridNode> findPathOnGrid(State state, int startX,
			int startY, int endX, int endY, Point[][] grid, int maxNodeCount) {
		System.out.println("Starting pathfinding...");

		// Check if the maxNodeCount is too small
		if (maxNodeCount <= 0) {
			return new ArrayList<GridNode>();
		}

		TreeSet<GridNode> openList = new TreeSet<GridNode>();
		List<GridNode> closedList = new ArrayList<GridNode>();

		// Find the grid points that are closest to the start and end points
		Point startPoint = new Point(startX, startY);
		Point endPoint = new Point(endX, endY);
		GridNode closestToStart = null;
		GridNode closestToEnd = null;
		for (int row = 0; row < grid[0].length; row++) {
			for (int col = 0; col < grid.length; col++) {
				if (grid[col][row] != null) {
					Point current = grid[col][row];

					if (closestToStart == null
							|| current.distance(startPoint) < closestToStart
									.getPoint().distance(startPoint)) {
						closestToStart = new GridNode(current.getX(), current
								.getY(), null, endX, endY, col, row);
					}
					if (closestToEnd == null
							|| current.distance(endPoint) < closestToEnd
									.getPoint().distance(endPoint)) {
						closestToEnd = new GridNode(current.getX(), current
								.getY(), null, endX, endY, col, row);
					}
				}
			}
		}

		GridNode currentNode = closestToStart;

		// Add the first node to the open list
		openList.add(currentNode);

		int currentGridCol = 0;
		int currentGridRow = 0;

		while (openList.size() != 0) {
			// Find the node in the open list with the lowest F number & move it
			// to the closed list
			currentNode = openList.first();
			currentGridCol = currentNode.getGridCol();
			currentGridRow = currentNode.getGridRow();
			openList.remove(currentNode);

			closedList.add(currentNode);

			// Stop when you get close enough to the target
			if (currentNode.equals(closestToEnd)) {
				break;
			}

			// Check all the adjacent squares. Ignoring those that are on the
			// closed list or unwalkable. Add nodes to the open list if they are
			// not on the open list already. Make the last node the parent of
			// the new squares.

			// North West
			// Check if this node exists
			// verify the coordinates
			if (currentGridCol - 1 >= 0 && currentGridRow - 1 >= 0
					&& grid[currentGridCol - 1][currentGridRow - 1] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol - 1,
						currentGridRow - 1);
			}

			// North
			if (currentGridRow - 1 >= 0
					&& grid[currentGridCol][currentGridRow - 1] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol, currentGridRow - 1);
			}

			// North East
			if (currentGridCol + 1 < grid.length && currentGridRow - 1 >= 0
					&& grid[currentGridCol + 1][currentGridRow - 1] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol + 1,
						currentGridRow - 1);
			}

			// East
			if (currentGridCol + 1 < grid.length
					&& grid[currentGridCol + 1][currentGridRow] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol + 1, currentGridRow);
			}

			// South East
			if (currentGridCol + 1 < grid.length
					&& currentGridRow + 1 < grid[0].length
					&& grid[currentGridCol + 1][currentGridRow + 1] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol + 1,
						currentGridRow + 1);
			}

			// South
			if (currentGridRow + 1 < grid[0].length
					&& grid[currentGridCol][currentGridRow + 1] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol, currentGridRow + 1);
			}

			// South West
			if (currentGridCol - 1 >= 0 && currentGridRow + 1 < grid[0].length
					&& grid[currentGridCol - 1][currentGridRow + 1] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol - 1,
						currentGridRow + 1);
			}

			// West
			if (currentGridCol - 1 >= 0
					&& grid[currentGridCol - 1][currentGridRow] != null) {
				checkNewGridNode(grid, currentNode, endX, endY, openList,
						closedList, state, currentGridCol - 1, currentGridRow);
			}
		}

		// Build the path of nodes.
		List<GridNode> path = new ArrayList<GridNode>();
		// Node currentNode = target;
		while (currentNode != null) {
			path.add(currentNode);
			currentNode = currentNode.getParent();
		}

		// Check the last(first) node in the list. It must be the start point
		GridNode lastNode = path.get(path.size() - 1);
		if (lastNode.getX() != startX || lastNode.getY() != startY) {
			// Create a new GridNode to contain the start point
			GridNode startNode = new GridNode(startX, startY, null, endX, endY,
					-1, -1);

			// If adding this node will cause a strange "switchback", remove the
			// previous(next) node.
			if (path.size() > 1) {
				Point secondToLastNode = path.get(path.size() - 2).getPoint();

				if (secondToLastNode.distance(startNode.getPoint()) < secondToLastNode
						.distance(lastNode.getPoint())) {
					// remove the last(first) node
					path.remove(path.size() - 1);
				}
			}

			// add the start point to the end(start) of the path
			path.add(startNode);
		}

		// The path is created but in reverse order.
		// Reverse the list.
		Collections.reverse(path);

		// Trim down the final path to the maxNodeCount
		if (maxNodeCount < path.size()) {
			path = path.subList(0, maxNodeCount);
		}

		return path;
	}

	private static void checkNewGridNode(Point[][] grid, GridNode parent,
			int endX, int endY, TreeSet<GridNode> openList,
			List<GridNode> closedList, State state, int gridCol, int gridRow) {

		Point coords = grid[gridCol][gridRow];

		GridNode mysteryNode = new GridNode(coords.getX(), coords.getY(),
				parent, endX, endY, gridCol, gridRow);

		if (closedList.contains(mysteryNode)) {
			return;
		}

		// Check if the node is on the open list.
		boolean onOpenList = false;
		boolean replaceNeeded = false;
		// Get the version on the open list
		for (Iterator<GridNode> it = openList.iterator(); it.hasNext();) {
			GridNode current = it.next();
			if (current.equals(mysteryNode)) {
				onOpenList = true;
				// if the new node has a lower G score
				if (mysteryNode.getG() < current.getG()) {
					// replace with the lower scoring node
					it.remove();
					replaceNeeded = true;
				}
				break;
			}
		}
		// Must be added outside of loop to avoid confusing the iterator.
		if (replaceNeeded) {
			openList.add(mysteryNode);
		}

		if (!onOpenList && !closedList.contains(mysteryNode)) {
			// here we know the node is not on either list
			// add it to the open list
			openList.add(mysteryNode);
		}
	}
}
